#ifndef PRM_PLANNER_H
#define PRM_PLANNER_H

#include "Graph.h"
#include "Kd_tree_d.h"
#include <CGAL/Random.h>
#include <boost/shared_ptr.hpp>
#include "Sampler.h"
#include "CollisionDetector.h"
#include "GuiUtils.h"

typedef int LABEL;
const LABEL START_LABEL = 0;
const LABEL END_LABEL = 1;

struct cmpPoint_d {
    bool operator()(const Point_d& a, const Point_d& b) const {
        for (int i = 0; i < a.dimension(); i++)
        {
            if (a[i] < b[i])
                return true;
            if (a[i] > b[i])
                return false;
        }
        return false;
    }
};

class PrmPlanner
{
public:

    ////////////////////////
    // C'tors & D'tors
    ////////////////////////

    PrmPlanner(Room room, vector<Polygon_2> robot_polygons, Obstacles obstacles, int robot_num, vector<Conf> start_confs, vector<Conf> target_confs);

    ~PrmPlanner(void);

    ////////////////////////
    // modifiers
    ////////////////////////

    void run();
    MPATH getPath(){ return m_path;}

    ////////////////////////
    // helpers
    ////////////////////////

private:
    ///////////////////////
    // Data members
    ///////////////////////

    void update_path();
    void generate_roadmap(int number_vertices = 300, int k_nearest = 12, double eps = 0.5);
    vector<Conf> generate_sample();
    bool local_planner(int start_index, int target_index, double eps);
    bool is_colliding(int start_index, int target_index, double eps);
    bool is_colliding(vector<Conf> robots_conf);

    Scene*                      m_scene;
    vector<Polygon_2>           m_robot_polygons;
    Obstacles                   m_obstacles;            // polygonal obstacles + scenario boundaries
    vector<Conf>                m_start_confs, m_target_confs;
    Room                        m_room;
    int                         m_robot_num;
    MPATH                       m_path;                 // path section - robot - configuration

    Graph<LABEL>*               m_graph;                // Graph structure
    Kd_tree_d<Kernel_d>*        m_kd_tree;              // Kd-tree for nearest neighbor search

    vector<vector<Conf>>        m_vertices;             // Contains all the points in the configuration space
    map<vector<Conf>, Point_d>  m_expandMap;            // Maps between a point in the configuration space to the 2d dimention equivelent
    map<Point_d, LABEL, cmpPoint_d>         m_map;                  // Maps between any point to it's index

    vector<CollisionDetector*>	m_cols;                 // Collision detectors for each robot with obstacles
    vector<Obstacles>           m_robotObs;             // The other robots as obstacles
    vector<vector<CollisionDetector*>> m_colsRobots;    // Collision detectors for each robot with other robots
    vector<Sampler*>			m_samplers;             // Samplers for each robot
};

#endif //PRM_PLANNER_H